#include "tankmessage.h"

module RobotMessageDispatcherP {

	uses {
		interface Boot;
		interface SplitControl as AMControl;

		interface Receive as ReceiveMove;
		interface Receive as ReceiveSettings;
		
		interface Receive as ReceivePower;
		interface Receive as ReceiveServo;
		interface Receive as ReceiveTower;

		interface AMSend as SendResponse;

		interface Packet;


		interface MainMotorControl;
		interface TowerRotate;
		interface PeriphereControl;
		//...
//		interface Timer<TMilli> as SensorTimer;
		interface Leds;
	}
}

implementation {
//	message_t msgbuffer;
//	message_t *buffer = &msgbuffer;

	message_t sendbuffer;

	uint16_t seqnum = 0;

	uint8_t t_state = 0;


	event void Boot.booted() {
		call AMControl.start();
	}

	event void AMControl.startDone(error_t err) {
		if (err == SUCCESS) {
			t_state = 1;

		}
		else {
			call AMControl.start();
		}
	}

	event void AMControl.stopDone(error_t err) {
		// do nothing
	}
	
	event message_t* ReceiveMove.receive(message_t* bufPtr, void* payload, uint8_t len) {
		if (len == sizeof(tank_move_msg_t)) {
			tank_move_msg_t * msgdata = (tank_move_msg_t *)payload;
			if (msgdata->mode == MOVE_MODE_PWM) {
				call MainMotorControl.setPWM(msgdata->vleft, msgdata->vright, msgdata->time);
			} else if (msgdata->mode == MOVE_MODE_PID) {
				call MainMotorControl.setPID(msgdata->vleft, msgdata->vright, msgdata->time);
			}
		}
		return bufPtr;
	}

	event message_t* ReceiveSettings.receive(message_t* bufPtr, void* payload, uint8_t len) {
		if (len == sizeof(tank_pid_settings_msg_t)) {
			tank_pid_settings_msg_t * msgdata = (tank_pid_settings_msg_t *)payload;
			call MainMotorControl.setParameters(msgdata->Cp, msgdata->Ci, msgdata->dt);
		}
		return bufPtr;
	}

	event message_t* ReceivePower.receive(message_t* bufPtr, void* payload, uint8_t len) {
		if (len == sizeof(tank_power_msg_t)) {
			tank_power_msg_t * msgdata = (tank_power_msg_t *)payload;
//			call TankHardware.setAuxiliaryPower(msgdata->power);
			call PeriphereControl.setPeripherePower(msgdata->power);
		}
		return bufPtr;
	}
	
	
	
	event message_t* ReceiveTower.receive(message_t* bufPtr, void* payload, uint8_t len) {
		if (len == sizeof(tank_tower_msg_t)) {
			tank_tower_msg_t * msgdata = (tank_tower_msg_t *)payload;
			switch (msgdata->cmd) {
				case CMD_TURN_CENTER:
					call TowerRotate.turnCenter(msgdata->params.s_center.speed);
					break;
				case CMD_TURN_SPEED_TIME:
					call TowerRotate.turnST(msgdata->params.s_t.speed, msgdata->params.s_t.time);
					break;
				case CMD_TURN_SPEED_POS:
					call TowerRotate.turnPos(msgdata->params.s_pos.speed, msgdata->params.s_pos.position);
					break;
				case CMD_TURN_SPEED_RELPOS:
					call TowerRotate.turnRelPos(msgdata->params.s_relpos.speed, msgdata->params.s_relpos.relposition);
					break;
			}
		}
		return bufPtr;
	}

	event void TowerRotate.turnCenterDone(error_t success) {
	}
	
	event void TowerRotate.turnSTDone(error_t success) {
	}
	
	event void TowerRotate.turnPosDone(error_t success) {
	}
	
	event void TowerRotate.turnRelPosDone(error_t success) {
	}

	event void MainMotorControl.stopped() {
	
	}

	event message_t* ReceiveServo.receive(message_t* bufPtr, void* payload, uint8_t len) {
		tank_servo_msg_t * msgdata = (tank_servo_msg_t *)payload;
//		call TankHardware.setServos(msgdata->servo1, msgdata->servo2, msgdata->servo3);
		return bufPtr;
	}

	event void SendResponse.sendDone(message_t* bufPtr, error_t error) {

	}

}
